# from dtw import window
from loop_closure_f import *
import matplotlib.pyplot as plt
import math
import numpy as np
import mpl_toolkits.mplot3d #用于画3d轨迹
import scipy.io as scio
import imageio
import time
import scipy.signal as signal

# from dtw import *
import dtw
from scipy.stats import stats

from curve_line_segmentation_f import *
from pathlib import Path
from io_f import *
from compute_f import split_ts_seq, blh2xyz84, dtw_distance
from visualiz_f import *
from sample_entropy import SampEn
from pos_update import updatePos

path_data_dir = '../../数据/室内外跑车数据'
save_dir = './output/site2/F1'
# ori_filename = '/Multi_Sensor/2021-07-13-14-46-52/Orientation.txt'
# pos_filename = '/Multi_Navi/2021-07-13-14-46-52/Cal_Result.txt'
# mag_filename = '/Multi_Sensor/2021-07-13-14-46-52/MagP.txt'

# ori_filename = '/Multi_Sensor/2021-07-13-14-24-44/Orientation.txt'
# pos_filename = '/Multi_Navi/2021-07-13-14-24-44/Cal_Result.txt'
# mag_filename = '/Multi_Sensor/2021-07-13-14-24-44/Mag.txt'

ori_filename = '/Multi_Sensor/2021-12-07-14-41-56/Orientation.txt'
mag_filename = '/Multi_Sensor/2021-12-07-14-41-56/Mag.txt'
pos_filename = '/Multi_Navi/2021-12-07-14-41-56/INS_Result.txt'

ori_path_filename = path_data_dir + ori_filename
pos_path_filename = path_data_dir + pos_filename
mag_path_filename = path_data_dir + mag_filename
# path_filenames = Path(path_data_dir).resolve().glob("Orientation.txt")


if __name__ == "__main__":
    # gy = read_data_gy(ori_path_filename)
    pos = read_data_pos(pos_path_filename) #0 time, 1 lat, 2 lon, 3 h, 4 ori，5 pos_delta  10hz的输出频率
    # plt.plot(pos[:, 2],pos[:, 1], marker=',')
    # plt.show()
    mag = read_data_mag(mag_path_filename) #0 time, 1 magx, 2 magy, 3 magz, 4 mag_xyz 100hz的输出频率

    #磁补偿
    mag[:,1] = mag[:,1] - 95
    mag[:,2] = mag[:,2] - 47

    # pos[:,0] = pos[:,0] - pos[0,0]
    # pos[:,0] = pos[:,0] - pos[0,0]
    for i in range(pos.shape[0]): #BLH转XYZ
        # pos[i,1:4] = blh2xyz84(pos[i,1], pos[i,2], pos[i,3]) #0 time, 1 lat, 2 lon, 3 h, 4 ori，5 pos_delta, 6 magx, 7 magy, 8 magz, 9 mag_xyz
        pos[i,1:3] = BL2Gauss(pos[i,1] , pos[i,2])
    for i in range(pos.shape[0]-1, -1, -1): #以第一个点作为原点
        pos[i,1:4] = pos[i,1:4] - pos[0,1:4]

    pos_grid = grid_pos(pos)
    
    # plt.plot(pos[:, 2],pos[:, 1], marker=',')
    # plt.show()
    # exit()

    #画两幅图的
    # fig = plt.figure(figsize=(15, 5))
    # ax = fig.add_subplot(1, 2, 1)
    # ax.plot(pos[:,4],marker=',')
    # ax = fig.add_subplot(1, 2, 2)
    # ax.plot(pos_grid[:,4],marker=',')
    # plt.show()
    # mag = mag[1150:,:]
    # mag[:,0] = (mag[:,0] - mag[0,0])/1000
    mag[:,0] = mag[:,0]/1000
    pos_mag = split_ts_seq(pos_grid, mag)#0 time, 1 X, 2 Y, 3 Z, 4 ori, 5 magx, 6 magy, 7 magz, 8 mag_xyz  #将位置与磁通过时间关联起来

    pos_grid = pos_mag[:,1:]# 0 X, 1 Y, 2 Z, 3 ori, 4 magx, 5 magy, 6 magz, 7 mag_xyz
    #分割出来转弯点
    # data_seg = curve_line_segmentation(pos[:,4], pos[:,5], slide_w=slide_window) #这个不好用
    # print(data_seg)
    # corner = corner_detected(pos_grid) #这个识别转弯还行
    # vis_corner(corner, pos_grid, True, True)
    # time.sleep(200)
    # print(corner)
    
    pos_grid = pos_grid[32500:54300,:] #第一圈旗杆室外到车库再出来到旗杆
    # pos_grid = pos_grid[55500:-1000,:] #第二圈旗杆室外到车库再出来到旗杆

    # pos_grid = pos_grid[36050:53500,:]
    # pos_grid = pos_grid[55000:,:]
    # pos_grid = pos_grid[59000:-2000,:]
    # with open("./results/01-initial.txt","w") as file_DK:
    #     for i in range(len(pos_grid)):
    #         file_DK.write('%f %f\n'%(pos_grid[i, 0], pos_grid[i, 1]))
    # file_DK.close()
    # print('01-initial.txt Write Success')
    # exit()


    
    # plt.plot(pos_grid[:, 1],pos_grid[:, 0], marker=',')
    # plt.show()
    # exit()


    # pos_grid = pos_grid[50000:,:]
    pos_grid_copy = pos_grid.copy()
    # fig = plt.figure()
    # plt.axis('equal')
    # # plt.plot(pos_grid[5000:-1,4],marker=',')
    # plt.plot(pos_grid[:, 1],pos_grid[:, 0], marker=',',label="直线约束前")
    lineFilter(pos_grid, pos_grid_copy)
    # fig = plt.figure()
    # plt.axis('equal')
    # plt.plot(pos_grid[:, 1],pos_grid[:, 0], marker=',', label="直线约束后")
    # plt.show()
    # exit()


    # corner = corner_detected(pos_grid)
    # visualTrajAndMag(pos_grid)
    # exit()
    # input()
    # visualTrajLineAndMag(pos_grid,corner)

    # fig = plt.figure()
    # # plt.plot(pos_grid[5000:-1,4],marker=',')
    # plt.plot(pos_grid[:,1], pos_grid[:,0], marker=',')
    # plt.show(block=False)
    # plt.pause(60)

    # fig = plt.figure()
    # plt.axis('equal')
    # # plt.plot(pos_grid[5000:-1,4],marker=',')
    # plt.plot(pos_grid[:,1], pos_grid[:,0], marker=',')
    # plt.show()
    # plt.pause(60)

    ### 磁序列峰值检测 ###
    # indoor_idx = 0 #室内车库开始的位置
    # window_slide = 50 #长度为window_slide/10 m的滑窗
    # sim_loop = []
    # image_list = []
    # fig = plt.figure()
    # plt.ion()
    # for i in range(window_slide, len(pos_grid) - window_slide, int(window_slide/2)): #直接先留出一个窗口长度作为历史轨迹

    #     traj_now = pos_grid[i:i + window_slide,:]
    #     traj_history = pos_grid[:i,:]
    #     s1_mag = traj_now[:,4:7]
    #     s2_mag = traj_history[:,4:7]
    #     s1_pos = traj_now[:,0:3]
    #     s2_pos = traj_history[:,0:3]
    #     # dist = np.reshape(np.sum(s1_mag**2,axis=1),(s1_mag.shape[0],1))+ np.sum(s2_mag**2,axis=1)-2*s1_mag.dot(s2_mag.T)
    #     # alignmentOBE = dtw.dtw(dist, keep_internals=True, step_pattern=dtw.asymmetric,open_end=True,open_begin=True)
    #     x = traj_now[:, 7]
    #     plt.plot(x)
    #     plt.plot(signal.argrelextrema(x,np.greater)[0],x[signal.argrelextrema(x, np.greater)],'o')
    #     # plt.plot(signal.argrelextrema(-x,np.greater)[0],x[signal.argrelextrema(-x, np.greater)],'+')
    #     plt.show()
    #     plt.pause(0.4)
    #     plt.clf()

    # initGraphRelative(pos_grid)
    indoor_idx = 0 #室内车库开始的位置
    window_slide = 50 #长度为window_slide/10 m的滑窗
    sim_loop = []

    image_list = []
    fig = plt.figure()
    plt.ion()
    loop_positive = False

    for i in range(window_slide, len(pos_grid) - window_slide, int(window_slide/2)): #直接先留出一个窗口长度作为历史轨迹
        if loop_positive:
            loop_positive = False
            continue
        traj_now = pos_grid[i:i + window_slide,:]
        traj_history = pos_grid[:i,:]
        s1_mag = traj_now[:,4:7]
        s2_mag = traj_history[:,4:7]
        s1_pos = traj_now[:,0:3]
        s2_pos = traj_history[:,0:3]
        dist = np.reshape(np.sum(s1_mag**2,axis=1),(s1_mag.shape[0],1))+ np.sum(s2_mag**2,axis=1)-2*s1_mag.dot(s2_mag.T)
        alignmentOBE = dtw.dtw(dist, keep_internals=True, step_pattern=dtw.asymmetric,open_end=True,open_begin=True)
        # # alignmentOBE.plot(type="twoway",offset=0)
        # # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))
        # if not constraint(s1_pos[alignmentOBE.index1],s2_pos[alignmentOBE.index2]): #不满足约束条件就continue
        #     continue
        # tmp = [i+alignmentOBE.index1, indoor_idx+alignmentOBE.index2]
        # sim_loop.append(tmp)

        # s1 = pos_grid[i:i + window_slide,:]
        # entropy = SampEn(s1_mag, r= np.std(s1_mag), m = 8)
        # print(entropy)

        # magSequence3DTo1(s1,s1_mag)
        # s2 = pos_grid[:i,:]
        
        # s2_mag = pos_grid_mag3to1[:i*3]
        # print(i,i + window_slide,len(s1), len(s2_mag))
        
        # plt.show()
        # plt.pause(1)
        # plt.clf()
        # s1_mag = s1[:,7]
        # s1_mag = s2[:,7]
        # s1_pos = s1[:,0:3]
        # s2_pos = s2[:,0:3]
        # alignmentOBE = dtw.dtw(s1_mag, s2_mag,keep_internals=True,step_pattern=dtw.asymmetricP1,open_end=True,open_begin=True)
        # #出来的序列投票法选位置
        # alignmentIndexVote(alignmentOBE.index1, alignmentIndexS1)
        # alignmentIndexVote(alignmentOBE.index2, alignmentIndexS2)
        # print(alignmentOBE.index2, alignmentIndexS2)
        # input()
        # alignmentOBE.plot(type="twoway",offset=0)
        # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))

        
        if not constraint(pos_grid[i+alignmentOBE.index1],pos_grid[alignmentOBE.index2]):
            continue
        sim_loop.append([i+alignmentOBE.index1, alignmentOBE.index2])
        loop_positive = True
        # print(sim_loop)
        # visualLoopClosureOnce(pos_grid, [i+alignmentOBE.index1, alignmentOBE.index2], fig)
        # if len(sim_loop) > 1:
        #     updatePos(pos_grid,sim_loop,window_slide)
        

        
        # visualLoopClosureOnce(pos_grid, [i+alignmentOBE.index1, alignmentOBE.index2], fig)
        # plt.savefig('temp.png')
        # image_list.append(imageio.imread('temp.png'))
        plt.clf()

        # plt.ion()    
        # s1, s2 = tmp[0], tmp[1]
        # # fig = plt.figure(figsize=(15, 5))
        # ax = fig.add_subplot(1, 2, 1)
        
        # ax.plot(pos_grid[:s1[0],0],pos_grid[:s1[0],1], color='blue',marker=',',label="reference") #绘制整个的历史轨迹
        # ax.plot(pos_grid[s1,0],pos_grid[s1,1], color='red',marker=',',label="s1",linestyle='--') #绘制的闭环轨迹段s1
        # ax.plot(pos_grid[s2,0],pos_grid[s2,1], color='green',marker=',',label="s2",linestyle='--') #绘制的闭环轨迹段s2
        # ax.axis('equal')
        # plt.legend(loc="lower left")
        
        # # ax.plot(pos_grid[4000:s1[-1],0],pos_grid[4000:s1[-1],1], marker=',',label="reference")
        # # ax.plot(pos_grid[s1,0],pos_grid[s1,1], marker=',',label="s1")
        # # ax.plot(pos_grid[s2,0],pos_grid[s2,1], marker=',',label="s2")
        # ax = fig.add_subplot(1, 2, 2)
        # # ax.ticklabel_format(style='plain')
        # ax.plot(pos_grid[s1,7], marker=',')
        # ax.plot(pos_grid[s2,7], marker=',')
    # updatePos(pos_grid,sim_loop,window_slide)
    # for i in range(window_slide, len(pos_grid) - window_slide, int(window_slide/2)): #直接先留出一个窗口长度作为历史轨迹
    #     if loop_positive:
    #         loop_positive = False
    #         continue
    #     traj_now = pos_grid[i:i + window_slide,:]
    #     traj_history = pos_grid[:i,:]
    #     s1_mag = traj_now[::-1,4:7]
    #     s1_mag[:,0:2] = s1_mag[:,0:2]*-1
    #     s2_mag = traj_history[:,4:7]
    #     s1_pos = traj_now[:,0:3]
    #     s2_pos = traj_history[:,0:3]
    #     dist = np.reshape(np.sum(s1_mag**2,axis=1),(s1_mag.shape[0],1))+ np.sum(s2_mag**2,axis=1)-2*s1_mag.dot(s2_mag.T)
    #     alignmentOBE = dtw.dtw(dist, keep_internals=True, step_pattern=dtw.asymmetric,open_end=True,open_begin=True)
    #     # # alignmentOBE.plot(type="twoway",offset=0)
    #     # # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))
    #     # if not constraint(s1_pos[alignmentOBE.index1],s2_pos[alignmentOBE.index2]): #不满足约束条件就continue
    #     #     continue
    #     # tmp = [i+alignmentOBE.index1, indoor_idx+alignmentOBE.index2]
    #     # sim_loop.append(tmp)

    #     # s1 = pos_grid[i:i + window_slide,:]
    #     # entropy = SampEn(s1_mag, r= np.std(s1_mag), m = 8)
    #     # print(entropy)

    #     # magSequence3DTo1(s1,s1_mag)
    #     # s2 = pos_grid[:i,:]
        
    #     # s2_mag = pos_grid_mag3to1[:i*3]
    #     # print(i,i + window_slide,len(s1), len(s2_mag))
        
    #     # plt.show()
    #     # plt.pause(1)
    #     # plt.clf()
    #     # s1_mag = s1[:,7]
    #     # s1_mag = s2[:,7]
    #     # s1_pos = s1[:,0:3]
    #     # s2_pos = s2[:,0:3]
    #     # alignmentOBE = dtw.dtw(s1_mag, s2_mag,keep_internals=True,step_pattern=dtw.asymmetricP1,open_end=True,open_begin=True)
    #     # #出来的序列投票法选位置
    #     # alignmentIndexVote(alignmentOBE.index1, alignmentIndexS1)
    #     # alignmentIndexVote(alignmentOBE.index2, alignmentIndexS2)
    #     # print(alignmentOBE.index2, alignmentIndexS2)
    #     # input()
    #     # alignmentOBE.plot(type="twoway",offset=0)
    #     # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))

        
    #     # if not constraint(pos_grid[i+alignmentOBE.index1],pos_grid[alignmentOBE.index2]):
    #     #     continue
    #     # sim_loop.append([i+alignmentOBE.index1, alignmentOBE.index2])
    #     loop_positive = True
    #     # print(sim_loop)
    #     # visualLoopClosureOnce(pos_grid, [i+alignmentOBE.index1, alignmentOBE.index2], fig)
    #     # if len(sim_loop) > 1:
    #     #     updatePos(pos_grid,sim_loop,window_slide)
        

        
    #     visualLoopClosureOnce(pos_grid, [i+alignmentOBE.index1, alignmentOBE.index2], fig)
    #     # plt.savefig('temp.png')
    #     # image_list.append(imageio.imread('temp.png'))
    #     plt.clf()
    initGraphAdjVertexs(pos_grid, sim_loop)#生成图
    # imageio.mimsave('pic2.gif', image_list, duration=0.4)
    exit()
################
    indoor_idx = 0 #室内车库开始的位置
    window_slide = 200 #长度为window_slide/10 m的滑窗
    sim_loop = []

    alignmentIndexS1 = np.zeros(window_slide, dtype=int)
    alignmentIndexS2 = np.zeros(window_slide, dtype=int)
    s1_mag = np.zeros(window_slide*3, dtype=float) #用来存待检测窗口的N x 3 磁矩阵转N*3 向量
    pos_grid_mag3to1 = np.zeros(pos_grid.shape[0]*3, dtype=float) #放所有轨迹的磁N x 3 磁矩阵转N*3 向量
    magSequence3DTo1(pos_grid,pos_grid_mag3to1)
    # print(indoor_idx + window_slide, len(pos_grid) - window_slide, int(window_slide/2))
    # input()
    image_list = []
    fig = plt.figure()
    plt.ion()  
    for i in range(indoor_idx + window_slide, len(pos_grid) - window_slide, int(window_slide/2)): #直接先留出一个窗口长度作为历史轨迹
        # print(i)
        
        traj_now = pos_grid[i:i + window_slide,:]
        traj_history = pos_grid[indoor_idx:i,:]
        s1_mag = traj_now[:,7]
        s2_mag = traj_history[:,7]
        s1_pos = traj_now[:,0:3]
        s2_pos = traj_history[:,0:3]
        # dist = np.reshape(np.sum(X**2,axis=1),(X.shape[0],1))+ np.sum(X_train**2,axis=1)-2*X.dot(X_train.T)
        # alignmentOBE = dtw.dtw(s1_mag, s2_mag, keep_internals=True, step_pattern=dtw.asymmetric,open_end=True,open_begin=True)
        # # alignmentOBE.plot(type="twoway",offset=0)
        # # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))
        # if not constraint(s1_pos[alignmentOBE.index1],s2_pos[alignmentOBE.index2]): #不满足约束条件就continue
        #     continue
        # tmp = [i+alignmentOBE.index1, indoor_idx+alignmentOBE.index2]
        # sim_loop.append(tmp)

        # s1 = pos_grid[i:i + window_slide,:]
        # entropy = SampEn(s1_mag, r= np.std(s1_mag), m = 8)
        # print(entropy)
        a = np.std(s1_mag)
        b = np.var(s1_mag)

        plt.title(str(a)+"  "+ str(b))
        plt.plot(s1_mag, marker=',')

        # magSequence3DTo1(s1,s1_mag)
        # s2 = pos_grid[:i,:]
        
        # s2_mag = pos_grid_mag3to1[:i*3]
        # print(i,i + window_slide,len(s1), len(s2_mag))
        
        plt.show()
        plt.pause(1)
        plt.clf()
        # s1_mag = s1[:,7]
        # s1_mag = s2[:,7]
        # s1_pos = s1[:,0:3]
        # s2_pos = s2[:,0:3]
        # alignmentOBE = dtw.dtw(s1_mag, s2_mag,keep_internals=True,step_pattern=dtw.asymmetricP1,open_end=True,open_begin=True)
        # #出来的序列投票法选位置
        # alignmentIndexVote(alignmentOBE.index1, alignmentIndexS1)
        # alignmentIndexVote(alignmentOBE.index2, alignmentIndexS2)
        # print(alignmentOBE.index2, alignmentIndexS2)
        # input()
        # alignmentOBE.plot(type="twoway",offset=0)
        # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))
        # if not constraint(pos_grid[alignmentIndexS1],pos_grid[alignmentIndexS2]):
        #     continue
        # sim_loop.append([i+alignmentIndexS1, alignmentIndexS2])
        # print(sim_loop)
        
        # visualLoopClosureOnce(pos_grid, [i+alignmentIndexS1, alignmentIndexS2], fig)
        # plt.savefig('temp.png')
        # image_list.append(imageio.imread('temp.png'))
        # plt.clf()

        # plt.ion()    
        # s1, s2 = tmp[0], tmp[1]
        # # fig = plt.figure(figsize=(15, 5))
        # ax = fig.add_subplot(1, 2, 1)
        
        # ax.plot(pos_grid[:s1[0],0],pos_grid[:s1[0],1], color='blue',marker=',',label="reference") #绘制整个的历史轨迹
        # ax.plot(pos_grid[s1,0],pos_grid[s1,1], color='red',marker=',',label="s1",linestyle='--') #绘制的闭环轨迹段s1
        # ax.plot(pos_grid[s2,0],pos_grid[s2,1], color='green',marker=',',label="s2",linestyle='--') #绘制的闭环轨迹段s2
        # ax.axis('equal')
        # plt.legend(loc="lower left")
        
        # # ax.plot(pos_grid[4000:s1[-1],0],pos_grid[4000:s1[-1],1], marker=',',label="reference")
        # # ax.plot(pos_grid[s1,0],pos_grid[s1,1], marker=',',label="s1")
        # # ax.plot(pos_grid[s2,0],pos_grid[s2,1], marker=',',label="s2")
        # ax = fig.add_subplot(1, 2, 2)
        # # ax.ticklabel_format(style='plain')
        # ax.plot(pos_grid[s1,7], marker=',')
        # ax.plot(pos_grid[s2,7], marker=',')
        
        


    fig = plt.figure()
    plt.ion()  
    for i in range(indoor_idx + window_slide, len(pos_grid) - window_slide, int(window_slide/2)):
        # print(i)
        
        # traj_now = pos_grid[i:i + window_slide,:]
        # traj_history = pos_grid[indoor_idx:i,:]
        # s1_mag = traj_now[:,7]
        # s2_mag = traj_history[:,7]
        # s1_pos = traj_now[:,0:3]
        # s2_pos = traj_history[:,0:3]
        # alignmentOBE = dtw.dtw(s1_mag, s2_mag, keep_internals=True, step_pattern=dtw.asymmetric,open_end=True,open_begin=True)
        # # alignmentOBE.plot(type="twoway",offset=0)
        # # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))
        # if not constraint(s1_pos[alignmentOBE.index1],s2_pos[alignmentOBE.index2]): #不满足约束条件就continue
        #     continue
        # tmp = [i+alignmentOBE.index1, indoor_idx+alignmentOBE.index2]
        # sim_loop.append(tmp)

        s1 = pos_grid[i:i + window_slide,:]
        magSequence3DTo1(s1,s1_mag)
        # s2 = pos_grid[:i,:]
        
        s2_mag = pos_grid_mag3to1[:i*3]
        # print(i,i + window_slide,len(s1), len(s2_mag))
        
        
        # s1_mag = s1[:,7]
        # s1_mag = s2[:,7]
        # s1_pos = s1[:,0:3]
        # s2_pos = s2[:,0:3]
        alignmentOBE = dtw.dtw(s1_mag, s2_mag,keep_internals=True,step_pattern=dtw.asymmetricP1,open_end=True,open_begin=True)
        # #出来的序列投票法选位置
        alignmentIndexVote(alignmentOBE.index1, alignmentIndexS1)
        alignmentIndexVote(alignmentOBE.index2, alignmentIndexS2)
        # print(alignmentOBE.index2, alignmentIndexS2)
        # input()
        # alignmentOBE.plot(type="twoway",offset=0)
        # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))
        # if not constraint(pos_grid[alignmentIndexS1],pos_grid[alignmentIndexS2]):
        #     continue
        # sim_loop.append([i+alignmentIndexS1, alignmentIndexS2])
        # print(sim_loop)
        
        # visualLoopClosureOnce(pos_grid, [i+alignmentIndexS1, alignmentIndexS2], fig)
        # plt.savefig('temp.png')
        # image_list.append(imageio.imread('temp.png'))
        # plt.clf()

        # plt.ion()    
        # s1, s2 = tmp[0], tmp[1]
        # # fig = plt.figure(figsize=(15, 5))
        # ax = fig.add_subplot(1, 2, 1)
        
        # ax.plot(pos_grid[:s1[0],0],pos_grid[:s1[0],1], color='blue',marker=',',label="reference") #绘制整个的历史轨迹
        # ax.plot(pos_grid[s1,0],pos_grid[s1,1], color='red',marker=',',label="s1",linestyle='--') #绘制的闭环轨迹段s1
        # ax.plot(pos_grid[s2,0],pos_grid[s2,1], color='green',marker=',',label="s2",linestyle='--') #绘制的闭环轨迹段s2
        # ax.axis('equal')
        # plt.legend(loc="lower left")
        
        # # ax.plot(pos_grid[4000:s1[-1],0],pos_grid[4000:s1[-1],1], marker=',',label="reference")
        # # ax.plot(pos_grid[s1,0],pos_grid[s1,1], marker=',',label="s1")
        # # ax.plot(pos_grid[s2,0],pos_grid[s2,1], marker=',',label="s2")
        # ax = fig.add_subplot(1, 2, 2)
        # # ax.ticklabel_format(style='plain')
        # ax.plot(pos_grid[s1,7], marker=',')
        # ax.plot(pos_grid[s2,7], marker=',')
        
        # plt.show()
        # plt.pause(0.4)
        # plt.clf()
    # imageio.mimsave('pic2.gif', image_list, duration=0.4)
    # print(sim_loop)
    # visualLoopClosure(pos_grid,sim_loop)#绘制闭环处
    # time.sleep(200)

    # corner = corner_detected(pos_grid)
    # print(corner)
    # vis_corner(corner, pos_grid, False, True)

    

    # word = []
    # sim_loop = []
    # fig = plt.figure()
    # for i in range(corner.shape[0]-1):
    #     print(i)
    #     # start_s1, stop_s1 = corner[i-1,0], corner[i-1,1]
    #     start_s1, stop_s1 = corner[i,0], corner[i,1] #这是转弯
    #     # start_s1, stop_s1 = corner[i,1], corner[i+1,0] #这是直线
    #     if stop_s1 - start_s1 < 10:
    #         print('长度不够')
    #         continue

    #     s1 = pos_grid[start_s1:stop_s1,:]
    #     s2 = pos_grid[0:start_s1,:]
    #     s1_mag = np.zeros((s1.shape[0]*3,1), dtype=float)
    #     s2_mag = np.zeros((s2.shape[0]*3,1), dtype=float)
    #     alignmentIndexS1 = np.zeros((s1.shape[0]*3,1), dtype=float)
    #     alignmentIndexS2 = np.zeros((s2.shape[0]*3,1), dtype=float)

    #     magSequence3DTo1(s1,s1_mag)
    #     magSequence3DTo1(s2,s2_mag)
    #     # s1_mag = s1[:,7]
    #     # s1_mag = s2[:,7]
    #     s1_pos = s1[:,0:3]
    #     s2_pos = s2[:,0:3]
    #     alignmentOBE = dtw.dtw(s1_mag, s2_mag,keep_internals=True,step_pattern=dtw.asymmetric,open_end=True,open_begin=True)
    #     # #出来的序列投票法选位置
    #     alignmentIndexVote(alignmentOBE.index1, alignmentIndexS1)
    #     alignmentIndexVote(alignmentOBE.index2, alignmentIndexS2)
    #     # alignmentOBE.plot(type="twoway",offset=0)
    #     # print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))
    #     # if not constraint(s1_pos[alignmentOBE.index1],s2_pos[alignmentOBE.index2]):
    #         # continue
    #     sim_loop.append([start_s1+alignmentOBE.index1, alignmentOBE.index2])

    #     plt.ion()    
        
        
    #     ax = fig.add_subplot(1, 2, 1)
    #     ax.plot(s2[:,0],s2[:,1],marker=',', label="traj")
    #     ax.plot(s1[:,0],s1[:,1],marker=',',label="s1")
    #     # ax.plot(pos_grid[start_s1:stop_s1,0],pos_grid[start_s1:stop_s1,1],marker=',',label="s2")
    #     plt.legend(loc="lower left")
    #     ax = fig.add_subplot(1, 2, 2)
    #     ax.plot(s1_mag,marker=',')
    #     ax.plot(pos_grid[:stop_s1,4],marker=',')
    #     plt.show()
    #     plt.pause(0.4)
        # plt.clf()


        #画两幅图的
    #     fig = plt.figure(figsize=(15, 5))
    #     ax = fig.add_subplot(1, 2, 1)
    #     ax.plot(pos_grid[4000:start_s1,0],pos_grid[4000:start_s1,1],marker=',', label="traj")
    #     ax.plot(s1_pos[alignmentOBE.index2,0],s1_pos[alignmentOBE.index2,1],marker=',',label="s1")
    #     ax.plot(pos_grid[start_s1:stop_s1,0],pos_grid[start_s1:stop_s1,1],marker=',',label="s2")
    #     plt.legend(loc="lower left")
    #     ax = fig.add_subplot(1, 2, 2)
    #     ax.plot(s1_mag,marker=',')
    #     ax.plot(pos_grid[alignmentOBE.index2,4],marker=',')
    #     plt.show()
    # plt.pause(600)
    # for i in range(10,corner.shape[0]):
    #     print(i)
    #     # start_s1, stop_s1 = corner[i-1,0], corner[i-1,1]
    #     # start_s1, stop_s1 = corner[i,0], corner[i,1] #这是转弯
    #     start_s1, stop_s1 = corner[i,0], corner[i+1,1] #这是直线
    #     if stop_s1 - start_s1 < 10:
    #         continue

    #     s1 = pos_grid[start_s1:stop_s1,:]
    #     s2 = pos_grid[0:start_s1,:]
    #     s1_mag = s1[:,4]
    #     s2_mag = s2[:,4]
    #     s1_pos = s1[:,0:3]
    #     s2_pos = s2[:,0:3]
    #     alignmentOBE = dtw(s1_mag, s2_mag,keep_internals=True,step_pattern=asymmetric,open_end=True,open_begin=True)
    #     # alignmentOBE.plot(type="twoway",offset=0)
    #     print('%d,%d'%(len(alignmentOBE.index1),len(alignmentOBE.index2)))
    #     if not constraint(s1_pos[alignmentOBE.index1],s2_pos[alignmentOBE.index2]):
    #         continue
    #     sim_loop.append([start_s1+alignmentOBE.index1, alignmentOBE.index2])
    # print(sim_loop)

    # visualLoopClosure(pos_grid,sim_loop)#绘制闭环处

    # for loop_closure in sim_loop:
    #     for i in range(len(loop_closure)):
    #         idfrom_idto = np.array([loop_closure[0][i],loop_closure[1][i]])
    # print(sim_loop)

    # fig = plt.figure(figsize=(15, 5))
    # plt.plot(pos_grid[4000:-1,3],marker=',')
    # plt.show(block=False)

    initGraph(pos_grid,sim_loop)#生成图

    # plt.pause(30)
        # plt.plot(alignmentOBE.index1, alignmentOBE.index2)s
    # for i in range(10,corner.shape[0]):
    #     print(i)
    #     start_s1, stop_s1 = corner[i-1,0], corner[i-1,1]
    #     start_s2, stop_s2 = corner[i,0], corner[i,1]
    #     s1 = pos_grid[0:stop_s2,4]
    #     s2 = pos_grid[start_s2:stop_s2,4]
    #     match_index_left = sequenceMatch(s1,s2)
    #     sim_loop.append([match_index_left,match_index_left-start_s2+stop_s2,start_s2, stop_s2])

    
    # word = []
    # sim_loop = []
    # for i in range(10,corner.shape[0]):
    #     print(i)
    #     start_s1, stop_s1 = corner[i,0], corner[i,1]
    #     s1 = pos_grid[start_s1 + 3:stop_s1+1 - 3,7]
    #     if s1.size != 15: continue
    #     for [start_s2, stop_s2] in word:
    #         # print([start_s2, stop_s2])
    #         s2 = pos_grid[start_s2:stop_s2+1,7]
    #         # print(len(s2))
    #         for j in range(5):
    #             s2_tmp = s2[j:15+j]
    #             if len(s2_tmp) != 15: continue
    #             r,p_value = stats.pearsonr(s1,s2_tmp)
    #             # L_dis = min(sum(s1-s2_tmp),sum(s1 - s2_tmp[::-1]))
    #             # print(r)
    #             if r > 0.8:
    #                 # print(r)
    #                 # L_dis = min(sum(s1-s2_tmp),sum(s1 - s2_tmp[::-1]))
    #                 tmp_dis = min(dtw_distance(s1,s2), dtw_distance(s1,s2[::-1]))
    #                 if tmp_dis > 40 : continue
    #                 x_s1 = sum(pos_grid[start_s1+j:start_s1+j+15,0])/15
    #                 y_s1 = sum(pos_grid[start_s1+j:start_s1+j+15,1])/15
    #                 x_s2 = sum(pos_grid[start_s2:stop_s2+1,0])/15
    #                 y_s2 = sum(pos_grid[start_s2:stop_s2+1,1])/15
    #                 radius = math.sqrt((x_s1-x_s2)**2 + (y_s1-y_s2)**2)
    #                 print(radius)
    #                 if radius > 20: continue
    #                 sim_loop.append([start_s1+j , start_s1+j+15,start_s2, stop_s2,r])
    #                 # fig = plt.figure(figsize=(1, 2))
    #                 # plt.ion()
    #                 # ax = fig.add_subplot(1, 2, 1)
    #                 # ax.plot(pos_grid[start_s1 + 3:stop_s1+1 - 3,0],pos_grid[start_s1 + 3:stop_s1+1 - 3,1], marker=',')
    #                 # ax.plot(pos_grid[start_s2+j:stop_s2+1-j,0],pos_grid[start_s2+j:stop_s2+1-j,1], marker=',')
    #                 # ax = fig.add_subplot(1, 2, 2)
    #                 # ax.plot(s1)
    #                 # ax.plot(s2_tmp)
    #                 # plt.show()
    #                 # plt.clf()
    #                 # input()
    #     word.append([start_s1, stop_s1])

    # print(sim_loop)
    # fig = plt.figure(figsize=(15, 5))
    # plt.ion()
    # image_list = []
    # for each in sim_loop:
    #     start_s1, stop_s1 = each[0], each[1]
    #     start_s2, stop_s2 = each[2], each[3]
    #     ax = fig.add_subplot(1, 2, 1)
    #     ax.plot(pos_grid[start_s2:stop_s1+1 - 3,0],pos_grid[start_s2:stop_s1+1 - 3,1], marker=',')
    #     ax.plot(pos_grid[start_s1 + 3:stop_s1+1 - 3,0],pos_grid[start_s1 + 3:stop_s1+1 - 3,1], marker=',')
    #     ax.plot(pos_grid[start_s2:stop_s2+1,0],pos_grid[start_s2:stop_s2+1,1], marker=',')
    #     ax = fig.add_subplot(1, 2, 2)
    #     ax.plot(pos_grid[start_s1 + 3:stop_s1+1 - 3,7], marker=',')
    #     ax.plot(pos_grid[start_s2:stop_s2+1,7], marker=',')
    #     plt.savefig('temp.png')
    #     image_list.append(imageio.imread('temp.png'))
    #     plt.show()
    #     plt.pause(1)
    #     plt.clf()
    # imageio.mimsave('pic2.gif', image_list, duration=1)        
            # tmp_dis = min(dtw_distance(s1,s2), dtw_distance(s1,s2[::-1]))
            # print(tmp_dis)
            # if tmp_dis < 25:
            #     # plt.plot(pos_grid[start:stop+1,0],pos_grid[start:stop+1,1], marker=',')
            #     # plt.plot(word[j][:,0],word[j][:,1], marker=',')
            #     id = np.array(range(start_s2,stop_s1+1))[:,np.newaxis]
            #     id.shape[0]
            #     b = np.hstack((pos_grid[start_s2:stop_s1+1,0:2],pos_grid[start_s2:stop_s1+1,3][:,np.newaxis]))
            #     b.shape[0]
            #     vertices = np.concatenate((id,b),axis=1)
            #     weight = np.array([20.000000,0.000000,20.000000,100000.000,0.000000,0.000000]) #协方差信息矩阵，暂定这样的懒得算了
            #     edges = []
            #     for i in range(1,len(vertices)):
            #         trans = vertices[i,1:] - vertices[i-1,1:]
            #         idfrom_idto = np.array([vertices[i,0],vertices[i-1,0]])
            #         edges.append(np.hstack((idfrom_idto,trans,weight)))
            #     for _ in range(min(stop_s1-start_s1,stop_s2-start_s2)):
            #         trans = np.array([0,0,0])
            #         idfrom_idto = np.array([start_s1,start_s2])
            #         edges.append(np.hstack((idfrom_idto,trans,weight)))
            #         start_s1,start_s2 = start_s1+1,start_s2+1
            #     edges = np.array(edges)
            #     scio.savemat('car_e.mat', {'edge':edges})
            #     scio.savemat('car_v.mat', {'vertices':vertices})
            #     plt.plot(pos_grid[start_s2:stop_s1+1,0],pos_grid[start_s2:stop_s1+1,1], marker=',')
            #     plt.show()
        
    # word = []
    # for i in range(10,corner.shape[0]-1):
    #     print(i)
    #     start_s1, stop_s1 = corner[i,1], corner[i+1,0]
    #     s1 = pos_grid[start_s1:stop_s1+1,7]
    #     if s1.size < 10: continue
    #     for [start_s2, stop_s2] in word:
    #         s2_all = pos_grid[start_s2:stop_s2+1,:]
    #         s2 = s2_all[:,7]
    #         tmp_dis = min(dtw_distance(s1,s2), dtw_distance(s1,s2[::-1]))
    #         print(tmp_dis)
    #         if tmp_dis < 25:
    #             # plt.plot(pos_grid[start:stop+1,0],pos_grid[start:stop+1,1], marker=',')
    #             # plt.plot(word[j][:,0],word[j][:,1], marker=',')
    #             id = np.array(range(start_s2,stop_s1+1))[:,np.newaxis]
    #             id.shape[0]
    #             b = np.hstack((pos_grid[start_s2:stop_s1+1,0:2],pos_grid[start_s2:stop_s1+1,3][:,np.newaxis]))
    #             b.shape[0]
    #             vertices = np.concatenate((id,b),axis=1)
    #             weight = np.array([20.000000,0.000000,20.000000,100000.000,0.000000,0.000000]) #协方差信息矩阵，暂定这样的懒得算了
    #             edges = []
    #             for i in range(1,len(vertices)):
    #                 trans = vertices[i,1:] - vertices[i-1,1:]
    #                 idfrom_idto = np.array([vertices[i,0],vertices[i-1,0]])
    #                 edges.append(np.hstack((idfrom_idto,trans,weight)))
    #             for _ in range(min(stop_s1-start_s1,stop_s2-start_s2)):
    #                 trans = np.array([0,0,0])
    #                 idfrom_idto = np.array([start_s1,start_s2])
    #                 edges.append(np.hstack((idfrom_idto,trans,weight)))
    #                 start_s1,start_s2 = start_s1+1,start_s2+1
    #             edges = np.array(edges)
    #             scio.savemat('car_e.mat', {'edge':edges})
    #             scio.savemat('car_v.mat', {'vertices':vertices})
    #             plt.plot(pos_grid[start_s2:stop_s1+1,0],pos_grid[start_s2:stop_s1+1,1], marker=',')
    #             plt.show()
    #     word.append([start_s1, stop_s1])

    # dis = []
    # loop_sim = []
    # fig = plt.figure(figsize=(1, 2))
    # plt.ion()
    # for i in range(len(word)):
    #     tmp_distance = []
    #     tmp_loop_sim = []
    #     s1 = word[i]
        
    #     for j in range(i+1,len(word)):
    #         s2 = word[j]
    #         # tmp_dis1 = dtw(s1,s2,keep_internals=True,open_end=True,open_begin=True)
    #         # tmp_dis2 = dtw(s1,s2[::-1],keep_internals=True,step_pattern=asymmetric,open_end=True,open_begin=True)
    #         # tmp_distance.append(min(tmp_dis1.distance,tmp_dis2.distance))
    #         tmp_dis = min(dtw_distance(s1,s2), dtw_distance(s1,s2[::-1]))
    #         tmp_distance.append(tmp_dis)
    #         r,p_value = stats.pearsonr(s1,s2)
    #         if tmp_dis < 25:
    #             if r > 0.7:
    #                 loop_sim.append([corner[i,0], corner[i,1], corner[j,0], corner[j,1],tmp_dis])
    #             print([r,p_value])
    #             # ax = fig.add_subplot(1, 2, 1)
    #             # a = corner[i,0]
    #             # d = corner[j,1]
    #             # ax.plot(pos_grid[a:d+1,0],pos_grid[a:d+1,1], marker=',')
    #             # ax = fig.add_subplot(1, 2, 2)
    #             # ax.plot(s1)
    #             # ax.plot(s2)
    #             # plt.pause(2)
    #             # plt.show()
    #             # plt.clf()
    #     dis.append([tmp_distance,r,p_value])
    # print(dis)
    # print(loop_sim)

    # loop_sim = np.array(loop_sim)
    # fig = plt.figure(figsize=(15, 8))
    # for i in range(min(12,len(loop_sim))):
    #     a = loop_sim[i,0]
    #     b = loop_sim[i,1]
    #     c = loop_sim[i,2]
    #     d = loop_sim[i,3]
    #     # print(i)
    #     ax = fig.add_subplot(3, 4, 1 + i)
    #     ax.plot(pos_grid[a:d+1,0],pos_grid[a:d+1,1], marker=',')
    # plt.show()

    # scio.savemat('saveddata.mat', {'xi': xi,'yi': yi,'ui': ui,'vi': vi})